//gnd->gnd
//vcc->5vcc
//scl->A5
//sda->A4
#include <Wire.h>
#include <Servo.h>
#include "SDuFAT.h"

// boussole
int HMC6352Address = 0x42;
// This is calculated in the setup() function
int slaveAddress;
int ledPin = 13;
boolean ledState = false;
byte headingData[2];
int i, headingValue;

//GPS
#define GPM 0x68  	                              // GPM I2C Address

byte Address;                                         // Global variable
byte Data;                                            // Global variable

//SD
// define the pin that powers up the SD card
#define MEM_PW 8

// help string to be sent to the serial port
#define HELP "H help\nL file info\nD delete\nP append string\nW init file and write\nR dump to serial\nA append text\n"

// variable used when reading from serial
byte inSerByte = 0;

//servo
Servo myservo;  // create servo object to control a servo
                // a maximum of eight servo objects can be created
 
int pos = 0;    // variable to store the servo position
 

void setup()
{
//boussole
// Shift the device's documented slave address (0x42) 1 bit right
// This compensates for how the TWI library only wants the
// 7 most significant bits (with the high bit padded with 0)
//alimentation:
  pinMode(16, OUTPUT);
  digitalWrite(16, LOW);
  pinMode(17, OUTPUT);
  digitalWrite(17, HIGH);
slaveAddress = HMC6352Address >> 1;   // This results in 0x21 as the address to pass to TWI
Serial.begin(9600);
pinMode(ledPin, OUTPUT);      // Set the LED pin as output
Wire.begin();
  
//GPS
  delay(100);
  Serial.println("Application started...awaiting GPS data..");

  delay(3000);                                        // Wait 3 seconds for DS-GPM.S to initialise

//SD
  // on my MicroSD Module the power comes from a digital pin
  // I activate it at all times
  pinMode(MEM_PW, OUTPUT);
  digitalWrite(MEM_PW, HIGH);
  
//servo
  myservo.attach(2);  // attaches the servo on pin 9 to the servo object
}

 

int GetDouble(){				      // Get double register value from GPM

  int Value = 0; 
  byte H_Byte = 0;
  byte L_Byte = 0;

  Wire.beginTransmission(GPM);
  Wire.send(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 2);			      // Request double register
  while(Wire.available() < 2);			      // Wait for double byte
  H_Byte = Wire.receive();                            // Store one
  L_Byte = Wire.receive();                            // Store two

  Value = (H_Byte * 10) + L_Byte;                     // Adjust for one byte

  return(Value);	                          
}


int GetSingle(){				      // Get single register value from GPM

  int Value = 0; 

  Wire.beginTransmission(GPM);
  Wire.send(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 1);			      // Request register
  while(Wire.available() < 1);			      // Wait for single byte
  Value = Wire.receive();                             // and store

  return(Value);	                          
}

 
void loop()
//servo
{
  for(pos = 0; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees
  {                                  // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  for(pos = 180; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees
  {                                
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  
//boussole
  // Flash the LED on pin 13 just to show that something is happening
  // Also serves as an indication that we're not "stuck" waiting for TWI data
  ledState = !ledState;
  if (ledState) {
    digitalWrite(ledPin,HIGH);
  }
  else
  {
    digitalWrite(ledPin,LOW);
  }
  // Send a "A" command to the HMC6352
  // This requests the current heading data
  Wire.beginTransmission(slaveAddress);
  Wire.send("A");              // The "Get Data" command
  Wire.endTransmission();
  delay(10);                   // The HMC6352 needs at least a 70us (microsecond) delay
  // after this command.  Using 10ms just makes it safe
  // Read the 2 heading bytes, MSB first
  // The resulting 16bit word is the compass heading in 10th's of a degree
  // For example: a heading of 1345 would be 134.5 degrees
  Wire.requestFrom(slaveAddress, 2);        // Request the 2 byte heading (MSB comes first)
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together
  Serial.print("Heading boussole:");
  Serial.print(int (headingValue / 10));     // The whole number part of the heading
  Serial.print(".");
  Serial.print(int (headingValue % 10));     // The fractional part of the heading
  Serial.println(" degrees");
  delay(500);
  
//GPS
for(int x=0; x <=2; x++){

  Serial.print("Time: ");
  Address = 0;                                        // Point to Hours register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(":");
  Address = 2;                                        // Point to Minutes register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(":");
  Address = 4;                                        // Point to Seconds register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.println(Data, DEC);                          // and print to PC

  Serial.print("Date: ");
  Address = 6;                                        // Point to Day register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print("/");
  Address = 8;                                        // Point to Month register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print("/");
  Address = 10;                                       // Point to Year 1 register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 12;                                       // Point to Year 2 register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.println(Data, DEC);                          // and print to PC
  delay(1000);                                        // Wait a second
  }

  Serial.print("Heading: ");
  Address = 44;                                       // Point to Heading 1 register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 46;                                       // Point to Heading 2 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(".");
  Address = 47;                                       // Point to Heading 3 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.println(Data, DEC);                          // and print to PC

  Serial.print("Speed: ");
  Address = 52;                                       // Point to Speed 1 register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 54;                                       // Point to Speed 2 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(".");
  Address = 55;                                       // Point to Speed 3 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.println(Data, DEC);                          // and print to PC

  delay(3000);                                        // Wait 3 seconds

  Serial.print("Latitide: ");
  Address = 14;                                       // Point to Latitude 1 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 15;                                       // Point to Latitude 2 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(" ");
  Address = 16;                                       // Point to Latitude 3 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 17;                                       // Point to Latitude 4 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(".");
  Address = 18;                                       // Point to Latitude 5 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 19;                                       // Point to Latitude 6 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 20;                                       // Point to Latitude 7 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 21;                                       // Point to Latitude 8 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 22;                                       // Point to Latitude character register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.println(Data, BYTE);                         // and print to PC

  Serial.print("Longitude: ");
  Address = 23;                                       // Point to Longitude 1 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 24;                                       // Point to Longitude 2 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 25;                                       // Point to Longitude 3 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(" ");
  Address = 26;                                       // Point to Longitude 4 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 27;                                       // Point to Longitude 5 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(".");
  Address = 28;                                       // Point to Longitude 6 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 29;                                       // Point to Longitude 7 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 30;                                       // Point to Longitude 8 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 31;                                       // Point to Longitude 9 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 32;                                       // Point to Longitude character register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.println(Data, BYTE);                         // and print to PC

  delay(3000);
  
//SD
  // Arduino expects one of a series of one-byte commands
  // you can get some help by sending an 'H' over the serial port
  if (Serial.available() > 0) {
    int result = 0;
    inSerByte = Serial.read();
    switch (inSerByte) {
    case 'H':
      Serial.println(HELP);
      result = 3; // special output for help message
      break;
    case 'L':
      result = SD.ls("hola.txt");
      break;
    case 'R':
      result = SD.cat("hola.txt");
      break;
    case 'W':
      result = SD.write("hola.txt");
      break;
    case 'A':
      result = SD.append("hola.txt");
      break;
    case 'P':
      result = SD.println("hola.txt","\nhola caracola");
      break;
    case 'D':
      result = SD.del("hola.txt");
      break;
    default:
      result = 2; // value for unknown operation
      break;
    }
    
    // print a status message for the last issued command
    // for help (result == 3) won't print anything
    if (result == 1) SD.printEvent(ERROR, "hola.txt");
    else if (result == 2) SD.printEvent(WARNING, "unknown command");
    else if (result == 0) SD.printEvent(SUCCESS, "hola.txt");
  }
}
